//
// This file is part of cpp-lib, a library of C++ functions and classes.
// Cpp-lib is copyright (C) 2004 and onwards, The CPL Interest Group.
// 
// This library is free software; you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.
// 
// This library is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
// General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this library (see the file COPYING); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
// 02111-1307  USA
//
// $Id$
//


#ifndef CPP_LIB_MATH_UTIL_H
#define CPP_LIB_MATH_UTIL_H

#include <exception>
#include <stdexcept>
#include <sstream>
#include <vector>
#include <limits>

#include <cassert>
#include <cmath>


namespace cpl {

namespace math     {

  
/// Guess what...

const double pi = 3.14159265358979323846264338327950288419716939937510 ;


/// Default precision (number of decimal digits) for writing to streams.
enum { DEFAULT_PRECISION = 16 } ;

//
// A sane modulo function.
//

inline double modulo( double const& x , double const& m ) {

  assert( m > 0 ) ;

  double const y = std::fmod( x , m ) ;

  if( y < 0 ) { return y + m ; }
  else        { return y     ; }

}



//
// Return an equivalent angle in [ 0 , 2pi).
//

inline double const angle_0_2pi( double const& a )
{ return modulo( a , 2 * pi ) ; }


/// \return The angle equivalent to a in [-pi , pi).
inline double const angle_mpi_pi( double const& a ) {

  double const aa = angle_0_2pi( a ) ;
  if( aa > pi ) { return aa - 2 * pi ; }
  else          { return aa          ; }
  
}



/// Sign function with optional threshold.

/// \return Sign of x or 0 if |\a x| <= \a threshold.

template< typename T >
inline int sign( T const& x , T const& threshold = 0 ) {

  assert( threshold >= 0 ) ;

  if( x < -threshold ) return -1 ;
  if( x >  threshold ) return  1 ;
  return 0 ;

}


/// Maximum of three values.

/// \return max( x1 , x2 , x3 ).

// Microsoft Compiler Version 13.10.3077 for 80x86 dies with fatal error
// C1001: INTERNAL COMPILER ERROR on this...
#ifndef _MSC_VER
template< typename T >
T max( T const& x1 , T const& x2 , T const& x3 ) {

  return std::max( x1 , std::max( x2 , x3 ) ) ;

}
#endif


/// Clamp value to a range.

/// \reval x Is set to \a lower if \a x < \a lower or \a upper if \a x >
/// \a upper.  Otherwise, \a x is left alone.
/// \precond \a lower <= \a upper.

template< typename T >
void clamp( T& x , T const& lower , T const& upper ) ;


/// Minimum of positive or undefined values.

/// \return min( \a x1 , \a x2 ), where negative values of the input
/// parameters stand for undefined values.  If one of the parameters
/// is negative, the other parameter is returned.

template< typename T >
T pos_min( T const& x1 , T const& x2 ) {

  if( x1 < 0 ) { return x2 ; }
  if( x2 < 0 ) { return x1 ; }

  assert( x1 >= 0 ) ;
  assert( x2 >= 0 ) ;

  return std::min( x1 , x2 ) ;

}


/// \return \a x if \a x is even, otherwise return \a x - 1.

template< typename T >
inline T even( T x ) {

  // assert( std::numeric_limits< T >::is_integer ) ;

  return x % 2 ? x - 1 : x ;

}


/// Next power of two.

/// \return The smallest integer p such that p is a power of two and x <= p.

long next_power_of_two( const long x ) ;


/// \return The dual logarithm of \a x.

int log_2( const long x ) ;


/// \return True if and only if \a x is a power of two.

inline bool is_power_of_two( const long x )
{ return cpl::math::next_power_of_two( x ) == x ; }


/// \return The bit reversal of \a x with its lower \a l bits reversed.

long bit_reversal( const long x , const int l ) ;


/// Perform bit-reversal permutation on [ begin , end ).

/// \pre \a ran_it must be a random access iterator.
/// \pre \a end - \a begin must be a power of two.
/// \post Each element x[ i ] is swapped with x[ bit_reversal( i ) ].

template< typename ran_it >
void bit_reversal_permutation( ran_it const& begin , ran_it const& end ) ;


/// \return x^2.

template< typename T >
inline T square( T const& x )
{ return x * x ; }


/// A simple statically expanded power function.

/// \return x^p

template< unsigned long p , typename T >
inline T power( T const& x ) {

  if( p == 0 ) return 1 ;

  T y = square( power< p / 2 >( x ) ) ;

  return ( p % 2 ) ? y * x : y ;

}


//
// 4th order Runge-Kutta integration step for 
//
//   d/dt x = f( x , u ) 
//
// for time step dt with u constant over dt.
//
// Requires that X have addition and scalar multiplication.
//

template< typename X , typename U , typename F >
inline X const rk_update
( X const& x , U const& u , F const& f , double const& dt ) {

  X const k1 = dt * f( x           , u ) ;
  X const k2 = dt * f( x + .5 * k1 , u ) ;
  X const k3 = dt * f( x + .5 * k2 , u ) ;
  X const k4 = dt * f( x +      k3 , u ) ;

  return x 
    + ( 1 / 6. ) * k1 
    + ( 1 / 3. ) * k2 
    + ( 1 / 3. ) * k3 
    + ( 1 / 6. ) * k4 
  ;

}

//
// Helper class as a default argument to solver step() function.
//

template< typename M > struct noop_inspector_type {

  void operator()( M const& , double const& ) const {}

} ;


template< typename T = double >
struct zero_crossing {

  zero_crossing() : fire( false ) {}

  typedef T discrete_state_type ;

  T const default_discrete_state() const { return 0 ; }

  void outputs( discrete_state_type const& x , T const& u ) { 

	fire =    u == 0 
	       || u > 0 && x < 0 
		   || u < 0 && x > 0 
    ;

  }

  void update_discrete_states( discrete_state_type& x , T const& u ) const
  { x = u ; }

  bool fire ;

} ;




//
// 4th order Runge-Kutta solver with discrete state update.  M must support
//
//   projection            ()
//   outputs               ()
//   update_discrete_states()
//   derivatives           () 
//
// which are called in turn in this order.
//
// M must supply type state_type supporting addition and scalar
// multiplication for the continuous states.
//
// M::default_state() must return a suitably initialized (dimensions,...)
// state_type object.
//
// M::default_discrete_state() must return a suitably initialized 
// (dimensions,...) discrete_state_type object for the discrete states.
//
//

template< typename M > struct rk4_solver {

  rk4_solver( M& m , double const& dt )
  : t ( 0  ) , 
	dt( dt ) , 
	m ( m  ) ,

	x  ( m.default_state() ) , 
	k1 ( m.default_state() ) , 
	k2 ( m.default_state() ) , 
	k3 ( m.default_state() ) , 
	k4 ( m.default_state() ) ,
	arg( m.default_state() ) ,

	xd ( m.default_discrete_state() )

	{}

  // Return model.
  M const& model() const { return m ; }
  M      & model()       { return m ; }

  // Return current time.
  double const& time() const { return t ; }

  // Set current time.
  void set_time( double const& t_ ) { t = t_ ; }

  // Return time step.
  double const& time_step() const { return dt ; }

  //
  // Advance model one time step.  Model input must have been set and
  // are assumed to be constant over dt.
  //
  // The expression f( m , t ) must be valid and can be used to
  // inspect the model variables at a precise point during the
  // integration, namely after the first output() call.  f() is passed a const
  // M& m, it's not allowed to modify variables!
  //

  template< typename F >
  void step( F const& f ) {

	m.projection( x ) ;

	m.outputs( x , xd ) ;
	m.update_discrete_states( xd ) ;

	f( model() , time() ) ;

	k1 = m.derivatives( x   ) ; k1 *= dt ;

	arg = k1 ; arg *= .5 ; arg += x ; 
	m.outputs( arg , xd ) ;
	k2 = m.derivatives( arg ) ; k2 *= dt ;
	
	arg = k2 ; arg *= .5 ; arg += x ; 
	m.outputs( arg , xd ) ;
	k3 = m.derivatives( arg ) ; k3 *= dt ;

	arg = k3 ;             arg += x ;
	m.outputs( arg , xd ) ;
	k4 = m.derivatives( arg ) ; k4 *= dt ;

	k1 *= .1666666666666666666666666666666666 ;
	k2 *= .3333333333333333333333333333333333 ;
	k3 *= .3333333333333333333333333333333333 ;
	k4 *= .1666666666666666666666666666666666 ;

	x += k1 ;
	x += k2 ;
	x += k3 ;
	x += k4 ;

	t += dt ;
	
  }

  // An no-op inspector that can be used as an argument to step().
  noop_inspector_type< M > noop_inspector() const 
  { return noop_inspector_type< M >() ; }

  // Return current state.
  typename M::state_type const& state() const { return x ; }
  typename M::state_type      & state()       { return x ; }

  typename M::discrete_state_type const& discrete_state() const { return xd ; }
  typename M::discrete_state_type      & discrete_state()       { return xd ; }

private:

  // Time.
  double t ;

  // Time delta.
  double dt ;

  // The model.
  M& m ;

  // The model state and temporary states.
  typename M::state_type x , k1 , k2 , k3 , k4 , arg ;

  // Model discrete state.
  typename M::discrete_state_type xd ;

} ;



/// Numerical derivative of a vector.

/// Compute numerical derivative of according to the symmetric formula
/// \f[
/// f^{ \prime }( x ) \approx \frac{ f( x + h ) - f( x - h ) }{ 2h }.
/// \f]
/// At the beginning and end of the vector, the asymmetric formula is
/// used.
///
/// \param v Tabulated function values, 1/\a f apart.
/// \param f Sampling frequency.
///
/// \return Tabulated function values of the derivative.  Same number of
/// elements as in \a v.

template< typename T >
std::vector< T >
derivative( std::vector< T > const& v , double const& f ) ;


/// Convert \a cont to std::vector< double >

/// \return Some sequence of some element type converted to a
/// double vector.

template< typename C >
inline std::vector< double >
to_double_vector( C const& cont ) {

  return std::vector< double >( cont.begin() , cont.end() ) ;

}


/// Report locations and values of Minima and Maxima in a vector.

/// The message format is:
///
///   name: maximum max at t = t_max , minimum min at t = t_min \n
///
/// \param os Stream to write to.
/// \param v The vector.
/// \param name The beginning of the message string.
/// \param frequency Sampling frequency, used for time
/// computation.

template< typename T >
void report_minmax(
  std::ostream& os ,
  std::vector< T > const& v ,
  std::string const& name ,
  long frequency
) ;


/// Sample a function.

/// Fill a container with sampled values, starting at t0 with frequency f.
///
/// \retval cont Where to write to.
/// \param function A unary function.
/// \param t0 The starting time.
/// \param f Frequency.

template< typename C , typename F , typename T >
void sample(
  C& cont ,
  F const& function ,
  T const& t0 ,
  T const& f
) ;


/// Multiply all elements of container \a c by \a x.

template< typename C >
void multiply( C& c , typename C::value_type const& x ) ;


/// assert() that all elements in range are >= 0

/// \param begin Iterator pointing to the beginning element of the range.
/// \param end Iterator pointing to the past-the-end element of the
/// range.

template< typename for_it >
void assert_nonneg( for_it const& begin , for_it const& end ) ;


/// Do downsampling.

/// Return a vector where each element is the mean of n succesive
/// elements of cont.  The cont.size() % n last elements of cont are
/// ignored.
///
/// \param cont The original value sequence.
/// \param n Downsampling factor.
/// \return The vector of downsampled values.

template< typename C >
std::vector< typename C::value_type >
down_sample( C const& cont , long n ) {

  assert( n >= 1 ) ;

  typedef typename C::value_type T ;

  std::vector< T > ret( cont.size() / n ) ;

  for( typename std::vector< T >::size_type i = 0 ; i < ret.size() ; ++i ) {

    T x = 0 ;

    for( long j = 0 ; j < n ; ++j )
      x += cont[ n * i + j ] ;

    ret[ i ] = x / n ;

  }

  return ret ;

}

} // namespace math

} // namespace cpl


//
// Template and inline definitions.
//

//
// Compute derivative.  See ``Numerical Recipes in C'', Section 5.7
// ``Numerical Derivatives'', (5.7.7).
//

template< typename T >
std::vector< T >
cpl::math::derivative( std::vector< T > const& v , double const& f ) {

  assert( f > 0 ) ;

  std::vector< T > ret( v.size() ) ;

  if( v.size() == 0 ) return ret ;

  if( v.size() == 1 ) { ret[ 0 ] = 0 ; return ret ; }


  for( typename std::vector< T >::size_type i = 1 ; i < v.size() - 1 ; ++i )

    ret[ i ] = f * ( v[ i + 1 ] - v[ i - 1 ] ) / 2 ;


  ret[ 0 ] = f * ( v[ 1 ] - v[ 0 ] ) ;

  ret.back() = f * ( *( v.end() - 1 ) - *( v.end() - 2 ) ) ;


  return ret ;

}



template< typename T >
void cpl::math::report_minmax(
  std::ostream& os ,
  std::vector< T > const& v ,
  std::string const& name ,
  long frequency
) {

  assert( v.size() ) ;

  typename std::vector< T >::const_iterator
    max = std::max_element( v.begin() , v.end() ) ;
  typename std::vector< T >::const_iterator
    min = std::min_element( v.begin() , v.end() ) ;

  os << name
     << ": maximum " << *max
     << " at t=" << static_cast< double >( max - v.begin() ) / frequency
     << ", minimum " << *min
     << " at t=" << static_cast< double >( min - v.begin() ) / frequency
     << '\n' ;

}


template< typename C , typename F , typename T >
void cpl::math::sample(
  C& c ,
  F const& fun ,
  T const& t0 ,
  T const& f
) {

  typename C::size_type j = 0 ;

  for( typename C::iterator i = c.begin() ; i != c.end() ; ++i )
    *i = fun( t0 + j++ / f ) ;

}


template< typename C >
void cpl::math::multiply( C& c , typename C::value_type const& x ) {

  for( typename C::iterator i = c.begin() ; i != c.end() ; ++i )
  { *i *= x ; }

}


template< typename for_it >
void cpl::math::assert_nonneg( for_it const& begin , for_it const& end ) {

  for( for_it i = begin ; i != end ; ++i )
    assert( *i >= 0 ) ;

}


template< typename ran_it >
void cpl::math::bit_reversal_permutation( ran_it const& begin , ran_it const& end ) {

  const long n = end - begin ;

  assert( n > 0 ) ;
  assert( is_power_of_two( n ) ) ;

  const int t = log_2( n ) ;

  if( t < 1 ) { return ; }

  for( long i = 0 ; i < n ; ++i ) {

    const long j = bit_reversal( i , t ) ;
    assert( 0 <= j && j < n ) ;

    if( j > i ) std::swap( *( begin + i ) , *( begin + j ) ) ;

  }

}


template< typename T >
void cpl::math::clamp( T& x , T const& lower , T const& upper ) {

  assert( lower <= upper ) ;
  if     ( x < lower ) { x = lower ; }
  else if( x > upper ) { x = upper ; }

}

#endif // CPP_LIB_MATH_UTIL_H
